#include <stdio.h>
#include <stdlib.h>
#include "robot.h"

/* Testea las funciones del capítulo 3:
 * -INVKIN
 */

int main(void)
{
    vec3 current={0.0, 0.0, 0.0};
    frame *wrelb = NULL;
    vec3 near;
    vec3 far;
    vec3 aux;
    vec3 dest={0.7,0.5,45.0};
    /* vec3 dest={-0.5,-0.5,50.0}; */
    Bool sol;
    frame *fr_near = NULL;
    frame *fr_far = NULL;
    
    wrelb = UTOI(dest);
    sol = INVKIN(wrelb, current, near, far);
    
    if (sol == TRUE)
        printf("Hay solución\n");
    else
    {
        printf("No hay solución\n");
        return 0;
    }
    
    fr_near = KIN(near);
    ITOU(fr_near, aux);
    printf("[%.3f,%.3f, %.3f]\n", aux[0], aux[1], aux[2]);
    fr_far = KIN(far);
    ITOU(fr_far, aux);
    printf("[%.3f,%.3f, %.3f]\n", aux[0], aux[1], aux[2]);
    
    /* Liberamos la memoria de los frames */
    gsl_matrix_free(wrelb);
    gsl_matrix_free(fr_far);
    gsl_matrix_free(fr_near);
    wrelb = NULL;
    fr_far = NULL;
    fr_near = NULL; 
    return 0;
}
